#!/usr/bin/python
# READY FOR MIT

import collections
import rospy
from std_msgs.msg import Float32, Float64
from sailing_robot.navigation import angle_average



class Wind_direction_average():
    def __init__(self):
        self.wind_direction_average_pub = rospy.Publisher('wind_direction_average', Float32, queue_size=10)

        rospy.init_node("sensor_processed_wind_direction", anonymous=True)
        rospy.Subscriber('heading', Float32, self.update_heading)
        self.heading = 0
        rospy.Subscriber('wind_direction_apparent', Float64, self.update_wind_direction_apparent)
        self.wind_direction_apparent = 0

        sensor_rate = rospy.get_param("config/rate")
        self.rate = rospy.Rate(sensor_rate)
        AVE_TIME = rospy.get_param("wind/trend_average_time")   # lengh of the averaging in seconds
        AVE_SIZE = int(AVE_TIME * sensor_rate)                        # size of the averaging sample
        self.average_list = collections.deque(maxlen = AVE_SIZE)
        self.wind_direction_average_publisher()


    def update_heading(self, msg):
        self.heading = msg.data


    def update_wind_direction_apparent(self, msg):
        self.wind_direction_apparent = msg.data


    def wind_direction_average_publisher(self):

        while not rospy.is_shutdown():
            wind_direction = (self.wind_direction_apparent + self.heading) % 360

            self.average_list.append(wind_direction)

            wind_direction_average = angle_average(list(self.average_list))
            self.wind_direction_average_pub.publish(wind_direction_average)

            self.rate.sleep()


if __name__ == '__main__':
    try:
        Wind_direction_average()
    except rospy.ROSInterruptException:
        pass
